#include "LRF.h"

LRF::LRF()
{
	for(int i = 0; i < LRF_SIZE; i++)
	{
		lrf[i] = LRF_DEFUALT_VALUE;
		//lrf_stat[i] = LRF_UNDEFINED;
	}

	//for(int i = 0; i < LRF_SIZE/2; i++)
	//	lrf2[i] = LRF2_DEFUALT_VALUE;
}

int LRF::write(int loc, unsigned char value)
{
	int rv = 0;
	if (loc >= 0 && loc < LRF_SIZE)
	{
		lrf[loc] = value;
		//lrf_stat[loc] = LRF_INT;
		rv = 1;
	}
	return rv;
}

int LRF::write(int loc, int value)
{
	int rv = 0;
	if (loc >= 0 && loc < LRF_SIZE)
	{
		lrf[loc] = value;
		lrf[loc+1] = value >> 8;
		lrf[loc+2] = value >> 16;
		lrf[loc+3] = value >> 24;
		//lrf_stat[loc] = LRF_INT;
		rv = 1;
	}
	return rv;
}

int LRF::write(int loc, float value)
{
	int rv = 0;
	//if (( loc % 2) == 1)
	//	loc = loc - 1;
	if (loc >= 0 && loc < LRF_SIZE/2)
	{
		lrf[loc] = value;
		//lrf2[loc/2] = value;
		//lrf_stat[loc] = LRF_FLOAT;
		//lrf_stat[loc+1] = LRF_FLOAT;
		rv = 1;
	}
	return rv;
}

int LRF::read(int loc, unsigned char& value)
{
	int rv = 0;
	if ((loc >= 0) && (loc < LRF_SIZE) /*&& (lrf_stat[loc] != LRF_FLOAT)*/)
	{
		value = lrf[loc];
		rv = 1;
	}
	return rv;
}

int LRF::read(int loc, int& value)
{
	int rv = 0;
	if ((loc >= 0) && (loc < LRF_SIZE) /*&& (lrf_stat[loc] != LRF_FLOAT)*/)
	{
		value = lrf[loc] | (lrf[loc+1] << 8) | (lrf[loc+2] << 16) | (lrf[loc+3] << 24);
		rv = 1;
	}
	return rv;
}

int LRF::read(int loc, float& value)
{
	int rv = 0;
	if ((loc >= 0) && (loc < LRF_SIZE/2) /*&& (lrf_stat[loc] != LRF_INT)*/)
	{
		//value = lrf2[loc];
		value = (float) lrf[loc];
		rv = 1;
	}
	return rv;
}

int LRF::clear()
{
	for(int i = 0; i < LRF_SIZE; i++)
	{
		lrf[i] = LRF_DEFUALT_VALUE;
		//lrf_stat[i] = LRF_UNDEFINED;
	}

	//for(int i = 0; i < LRF_SIZE/2; i++)
	//	lrf2[i] = LRF2_DEFUALT_VALUE;

	return 1;
}

